In recent years, studies about mixed reality that aims at seamless joint of real and virtual spaces have been extensively made. An image display apparatus that presents mixed reality is implemented by a video see-through system which superimposes and renders an image of a virtual space (e.g., virtual objects, text information, and the like rendered by computer graphics) generated in correspondence with the position and orientation of an image sensing device such as a video camera or the like on a real space image captured by the image sensing device, and displays these images.
In addition, the image display apparatus is implemented by an optical see-through system which displays a virtual space image generated in correspondence with the position and orientation of a viewpoint of an observer on an optical see-through display mounted on the observer's head.
As applications of such image display apparatus, new fields different from conventional virtual reality such as operation assistance that superimposes the state in a body onto the body surface of a patient, a mixed reality game in which a player fights against virtual enemies that swim on the real space, and the like are expected.
A common requirement for these applications involves the precision level of alignment between the real and virtual spaces, and many efforts have been conventionally made in this respect. A problem of alignment in mixed reality amounts to obtaining the position and orientation of an image sensing device in a scene (i.e., on a world coordinate system) in case of the video see-through system. Likewise, such problem amounts to obtaining the position and orientation of the viewpoint of the observer or display in a scene in case of the optical see-through system.
As a method of solving the former problem, it is a common practice to obtain the position and orientation of the image sensing device in the scene by laying out or setting a plurality of indicators in the scene, and detecting the coordinates of projected images of the indicators in an image captured by the image sensing device. As a method of solving the latter problem, it is a common practice to mount an image sensing device on an object to be measured (i.e., the observer's head or display), measure the position and orientation of the image sensing device by the same method as in the former method, and obtain the position and orientation of the object to be measured on the basis of the position and orientation of the image sensing device.
A conventional position/orientation measurement apparatus which measures the position and orientation of an image sensing device by detecting indicators from an image captured by the image sensing device will be explained below using FIG. 1. FIG. 1 is a diagram showing the arrangement of the conventional position/orientation measurement apparatus. As shown in FIG. 1, a conventional position/orientation measurement apparatus 100 comprises a subjective viewpoint indicator detection unit 110 and position/orientation calculation unit 120, and is connected to an image sensing device 130.
At a plurality of positions on a real space, a plurality of subjective viewpoint indicators Qk (k=1, . . . , K1) whose positions on a world coordinate system (a coordinate system specified by defining one point on the real space as an origin, and defining three orthogonal axes as the X-, Y-, and Z-axes) are known are laid out as indicators (to be referred to as subjective viewpoint indicators) to be captured by the image sensing device 130. The subjective viewpoint indicators Qk are preferably set so that at least three indicators are always observed on an image captured by the image sensing device 130 when the image sensing device 130 is located at respective points within the measurement range in which the position and orientation are to be measured. In the example shown in FIG. 1, four subjective viewpoint indicators Q1, Q2, Q3, and Q4 are laid out, and three indicators Q1, Q3, and Q4 of them are included in the field of view of the image sensing device 130.
The subjective viewpoint indicators Qk may comprise circular markers having different colors, or feature points such as natural features or the like having different texture features. However, the indicators to be used are not particularly limited as long as the image coordinates of projected images on a captured image can be detected, and the indicators can be identified from each other.
An image (to be referred to as a subjective viewpoint image hereinafter) output from the image sensing device 130 is input to the position/orientation measurement apparatus 100.
The subjective viewpoint indicator detection unit 110 receives the subjective viewpoint image from the image sensing device 130, and detects the image coordinates of the subjective viewpoint indicators Qk that appear in the input image. For example, when the subjective viewpoint indicators Qk comprise markers having different colors, regions corresponding to respective marker colors are detected from the subjective viewpoint image, and their barycentric positions are detected as the detected coordinates of the indicators. On the other hand, when the subjective viewpoint indicators Qk comprise feature points having different texture features, template matching using template images of the respective indicators, which are held as known information, is applied to the subjective viewpoint image, thereby detecting the positions of indicators.
Furthermore, the subjective viewpoint indicator detection unit 110 outputs an image coordinate uQkn and identifier kn of each detected subjective viewpoint indicator Qkn to the position/orientation calculation unit 120. Note that n (n=1, . . . , N) is an index corresponding to each detected indicator, and N is the total number of detected indicators. For example, in case of FIG. 1, N=3, and identifiers k1=1, k2=3, and k3=4, and corresponding image coordinates uQk1, uQk2, and uQk3 are output.
The position/orientation calculation unit 120 calculates the position and orientation of the image sensing device 130 on the basis of the relationship between the image coordinates uQkn of the detected subjective viewpoint indicators Qkn and world coordinates xWQkn of the indicators held in advance as known information.
The method of calculating the position and orientation of an image sensing device from the sets of world and image coordinates of subjective viewpoint indicators has been proposed as a long-established method in the field of phototopography and the like (e.g., see R. M. Haralick, C. Lee, K. Ottenberg, and M. Nolle: Review and analysis of solutions of the three point perspective pose estimation problem, International Journal of Computer Vision, vol. 13, no. 3, pp. 331–356, 1994. , and D. G. Lowe: Fitting parameterized three-dimensional models to images, IEEE Transactions on PAMI, vol. 13, no. 5, pp. 441–450, 1991.).
The process executed by the position/orientation calculation unit 120 of this prior art will be explained below using the flow chart of FIG. 2. In this prior art, the position and orientation of the image sensing device is calculated by a numerical calculation scheme based on recursive arithmetic operations.
The position/orientation calculation unit 120 internally expresses the position and orientation to be calculated of the image sensing device 130 as three-valued vectors [x y z]T and [ξ ψ ζ]T. Various methods of expressing the orientation by three values are available. In this case, the orientation is expressed by a three-valued vector that expresses the rotation angle by its size, and the rotation axis direction by its direction. At this time, unknown parameters to be calculated are described by a six-valued state vector s=[x y z ξ ψ ζ]T.
In step S2000, the position/orientation calculation unit 120 sets an appropriate initial value (estimated values of the position and orientation) in the state vector s. For example, s derived in the process of the previous frame (time tk-1) may be used as the initial value, or a value predicted based on a change in derived value from old “s”s may be set.
In step S2010, the position/orientation calculation unit 120 receives the actually measured values uQkn of the image coordinates and identifiers kn of the respective subjective viewpoint indicators Qkn detected by the subjective viewpoint indicator detection unit 110.
In step S2020, the position/orientation calculation unit 120 calculates estimated values uQkn* of the image coordinates of the respective subjective viewpoint indicators Qkn. The calculation of uQkn* is made based on a function of the world coordinates xWQkn of indicators Qkn held as known information and the current state vector s:
                              u                                    Q                              k                n                                      *                          =                              F            C                    ⁡                      (                                          x                W                                  Q                                      k                    n                                                              ,              s                        )                                              (        1        )            
More specifically, the function FC( ) comprises:
                              x          W                      Q                          k              n                                      =                              [                                                                                x                    C                                          Q                                              k                        n                                                                                                                                  y                    C                                          Q                                              k                        n                                                                                                                                  z                    C                                          Q                                              k                        n                                                                                                              1                                                      ]                    =                                                                      M                                      W                    ⁢                                                                                  ⁢                    C                                                  ⁡                                  (                  s                  )                                                            -                1                                      ·                          x              W                              Q                                  k                  n                                                                                        (        2        )            used to calculate a coordinate xcQkn on a camera coordinate system (a coordinate system specified by defining one point on the camera as an origin and defining three orthogonal axes as X-, Y-, and Z-axes) of the indicator of interest from xWQkn and s; and
                              u                                    Q                              k                n                                      *                          =                                            [                                                                                          u                      x                                                                        Q                                                      k                            n                                                                          *                                                                                                                        u                      y                                                                        Q                                                      k                            n                                                                          *                                                                                                        ]                        T                    =                      -                                          [                                                                            f                      x                      C                                        ⁢                                                                  x                        C                                                  Q                                                      k                            n                                                                                                                      z                        C                                                  Q                                                      k                            n                                                                                                                                -                                                            f                      y                      C                                        ⁢                                                                  y                        C                                                  Q                                                      k                            n                                                                                                                      z                        C                                                  Q                                                      k                            n                                                                                                                                              ]                            T                                                          (        3        )            used to calculate the image coordinate uQkn* from the camera coordinate xCQkn.Note that fCx and fCy are the focal lengths of the image sensing device 130 in the x- and y-axis directions, and are held as known values. Also, MWC(s) is a modeling conversion matrix (i.e., a matrix used to convert the coordinate on the camera coordinate system into that on the world coordinate system) determined by s, and is defined by:
                                                                                          M                  MC                                ⁡                                  (                  s                  )                                            =                              [                                                                                                                                                                                        ξ                              2                                                                                      θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          +                                                  cos                          ⁢                                                                                                          ⁢                          θ                                                                                                                                                                                                                        ξ                              ⁢                                                                                                                          ⁢                              ψ                                                                                      θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          -                                                                              ζ                            θ                                                    ⁢                          sin                          ⁢                                                                                                          ⁢                          θ                                                                                                                                                                                                                        ξ                              ⁢                                                                                                                          ⁢                              ζ                                                                                      θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          +                                                                              ψ                            θ                                                    ⁢                          sin                          ⁢                                                                                                          ⁢                          θ                                                                                                            x                                                                                                                                                                                ψξ                                                          θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          +                                                                              ζ                            θ                                                    ⁢                          sin                          ⁢                                                                                                          ⁢                          θ                                                                                                                                                                                                                        ψ                              2                                                                                      θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          +                                                  cos                          ⁢                                                                                                          ⁢                          θ                                                                                                                                                                                                                                                                                                                    ⁢                              ψζ                                                                                      θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          -                                                                              ξ                            θ                                                    ⁢                          sin                          ⁢                                                                                                          ⁢                          θ                                                                                                            y                                                                                                                                                                                                              ζξ                              ⁢                                                                                                                                                                                  θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          -                                                                              ψ                            θ                                                    ⁢                          sin                          ⁢                                                                                                          ⁢                          θ                                                                                                                                                                                          ζψ                                                          θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          +                                                                              ξ                            θ                                                    ⁢                          sin                          ⁢                                                                                                          ⁢                          θ                                                                                                                                                                                                                        ζ                              2                                                                                      θ                              2                                                                                ⁢                                                      (                                                          1                              -                                                              cos                                ⁢                                                                                                                                  ⁢                                θ                                                                                      )                                                                          +                                                  cos                          ⁢                                                                                                          ⁢                          θ                                                                                                            z                                                                                                  0                                                              0                                                              0                                                              1                                                                      ]                                                                                                        for                ⁢                                                                  ⁢                θ                            =                                                                                          ξ                      2                                        +                                          ψ                      2                                        +                                          ζ                      2                                                                      .                                                                        (        4        )            
In step S2030, the position/orientation calculation unit 120 calculates an error ΔuQkn between the estimated value uQkn* and actually measured value uQkn of the image coordinate of each subjective viewpoint indicator Qkn by:
                              Δ          ⁢                                          ⁢                      u                          Q                              k                n                                                    =                                  ⁢                              u                          Q                              k                n                                              -                                          ⁢                      u                                          Q                                  k                  n                                            *                                                          (        5        )            
In step S2040, the position/orientation calculation unit 120 calculates, for each subjective viewpoint indicator Qkn, a 2×6 Jacobian matrix JusQkn (=∂u/∂s) having, as elements, solutions obtained by partially differentiating the function FC( ) given by equation (1) by respective elements of the state vector s. More specifically, the unit 120 calculates a 2×3 Jacobian matrix JuxQkn(=∂u/∂x) having, as elements, solutions obtained by partially differentiating the right-hand side of equation (3) by elements of the camera coordinate xCQkn, and a 3×6 Jacobian matrix JxsQkn(=∂x/∂s) having, as elements, solutions obtained by partially differentiating the right-hand side of equation (2) by elements of the vector s, and then calculates JusQkn by:
                              J                      u            ⁢                                                  ⁢            s                                Q                          k              n                                      =                              J                          u              ⁢                                                          ⁢              x                                      Q                              k                n                                              ·                      J                          x              ⁢                                                          ⁢              s                                      Q                              k                n                                                                        (        6        )            
In step S2050, the position/orientation calculation unit 120 calculates a correction value Δs of the state vector s on the basis of the errors ΔuQkn and Jacobian matrices JusQkn calculated in steps S2030 and S2040. More specifically, the unit 120 generates a (2N)-dimensional error vector:
                    U        =                  [                                                                      Δ                  ⁢                                                                          ⁢                                      u                                          Q                                              k                        1                                                                                                                                                ⋮                                                                                      Δ                  ⁢                                                                          ⁢                                      u                                          Q                                              k                        N                                                                                                                          ]                                    (        7        )            by vertically lining up errors ΔuQkn, and a 2N×6 matrix:
                    Φ        =                  [                                                                      J                                      u                    ⁢                                                                                  ⁢                    s                                                        Q                                          k                      1                                                                                                                          ⋮                                                                                      J                                      u                    ⁢                                                                                  ⁢                    s                                                        Q                                          k                      N                                                                                                    ]                                    (        8        )            by vertically lining up Jacobian matrices JusQkn, and calculates the correction value Δs using a pseudo inverse matrix Φ′ of Φ by:ΔS=Φ′U  (9)Since N=3 in the example of FIG. 1, U is the six-dimensional vector, and Φ is the 6×6 matrix.
In step S2060, the position/orientation calculation unit 120 corrects the state vector s using the correction value Δs calculated in step S2050 by:S+ΔS→S  (10)and sets the corrected value as a new estimated value of s.
The position/orientation calculation unit 120 determines in step S2070 whether or not the calculation converges using some determination criteria, i.e., whether or not error vector U is smaller than a predetermined threshold value, whether or not the correction value Δs is smaller than a predetermined threshold value, and so forth. If the calculation does not converge, the unit 120 repeats the process in step S2020 and subsequent steps using the corrected state vector s.
In step S2080, the position/orientation calculation unit 120 outputs the obtained state vector s as the information of the position and orientation of the image sensing device 130. The information of the position and orientation is output in the form of, e.g., a viewing conversion matrix calculated from s.
In the conventional system, the position and orientation of the image sensing device 130 are acquired by the aforementioned method.
Also, the position and orientation of an object to be measured can also be calculated by setting a plurality of indicators on the object to be measured, capturing the object by an externally set objective viewpoint camera, and detecting image coordinates of projected images of the indicators in the captured objective viewpoint image (e.g., see D. G. Lowe: Fitting parameterized three-dimensional models to images, IEEE Transactions on PAMI, vol. 13, no. 5, pp. 441–450, 1991. , and Japanese Patent Laid-Open No. 09-323280).
However, with the former method that detects the indicators in the scene from the subjective viewpoint image obtained from the image sensing device as an object to be measured, since the moving amounts of the indicators on the image with respect to a change in position are smaller than those with respect to a change in orientation, sufficiently high position measurement precision cannot be obtained. In order to allow to look around over a broad range, many indicators must be set on the corresponding broad range region.
On the other hand, in the latter method that detects markers on an object to be measured from the objective viewpoint image obtained from the objective viewpoint camera, since the moving amounts of the indicators on the image with respect to a change in orientation are smaller than those with respect to a change in position, sufficiently high orientation measurement precision cannot be obtained.